/*
 * TJunctions_Slave.c
 *
 * Created: 6/3/2013 9:32:11 PM
 *  Author: Crossfire
 */


#include "TJunctions_Slave.h"

/**/uint8_t data = 0;

int main(void)
{
	DDRA = 0xFF;//??
	DDRB = 0xFF;
	DDRC = 0xFF;
	DDRD = 0xFF;
	TWI_init_slave(); // Function to initilaize slave
	while(1)
	{
		while((TWSR & 0xF8)!= 0x60)  // Loop till correct acknoledgement have been received
		{
			// Get acknowlegement, Enable TWI, Clear TWI interrupt flag
			TWCR=(1<<TWEA)|(1<<TWEN)|(1<<TWINT);
			while (!(TWCR & (1<<TWINT)))  // Wait for TWINT flag
			{
				//dostuff while waiting for I2C communication
				///set_state();
				///set_stoplichten(mode);
				/**/PORTB = ~data;
			}
		}
		/**/TWI_match_read_slave();
		/**/TWI_read_slave();
		/**/data = recv_data;
		///parse_data_tsplitsing();
		//PORTB = 0; //debug
		///_delay_ms(200);
		//PORTB = ~0; //debug
	}
}

void parse_data_tsplitsing()
{
	unsigned char data = 0;
	unsigned char save = 0;
	int shift = 6;
	int data_saved = 0;
	int amount_read = 0;
	TWI_match_read_slave();
	TWI_read_slave();
	mode = (recv_data & 1);
	TWI_match_read_slave();
	TWI_read_slave();
	data = recv_data;
	//
	for(int i = 0; i < 7; i++)
	{
		for (int j = 0; j < 3; i++)
		{
			save = ((data >> shift) & 3);
			TJunction[i][j] = save;
			shift -=2;
			if(shift == -2)
			{
				shift = 6;
				TWI_match_read_slave();
				TWI_read_slave();
				data = recv_data;
				amount_read++;
			}
		}
	}
	while(amount_read < 57)
	{
		amount_read++;
		TWI_match_read_slave();
		TWI_read_slave();
	}
}

// Function to initilaize slave
void TWI_init_slave(void)
{
	// Fill slave address to TWAR
	TWAR=TJUNCTION;
}

void TWI_read_slave(void)
{
	// Clear TWI interrupt flag,Get acknowlegement, Enable TWI
	TWCR= (1<<TWINT)|(1<<TWEA)|(1<<TWEN);
	// Wait for TWINT flag
	while (!(TWCR & (1<<TWINT)));
	// Wait for acknowledgement
	while((TWSR & 0xF8)!=0x80);
	// Get value from TWDR
	recv_data=TWDR;
	// send the receive value on PORTB
	//PORTB=recv_data;

}

//Function to match the slave address and slave dirction bit(read)
void TWI_match_read_slave(void)
{
	//Loop till correct acknoledgement have been received
	while((TWSR & 0xF8)!= 0x60)
	{
		//Get acknowlegement, Enable TWI, Clear TWI interrupt flag
		TWCR=(1<<TWEA)|(1<<TWEN)|(1<<TWINT);

		//Wait for TWINT flag
		while (!(TWCR & (1<<TWINT)));
	}
}

//Functie om verkeerslichten op de juiste modus te zetten, geen delay
void set_stoplichten(int mode)
{
	//Tijdelijke array om data in op te slaan
	uint8_t Temp_Array[7][3];

	//Als mode gelijk is aan 1, dan wordt de statemachine van de web applicatie afkomt gebruikt
	if(mode == 1)
	{
		for(int i = 0; i < 7; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				//Statemachine webapplicatie wordt in de tijdelijke array gezet
				Temp_Array[i][j] = TJunction[i][j];
			}
		}
	}
	//anders als mode gelijk is aan 0, dan wordt de default statemachine gebruikt
	else if(mode == 0)
	{
		for(int i = 0; i < 7; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				//Default statemachine wordt in de tijdelijke array gezet
				Temp_Array[i][j] = Default[i][j];
			}
		}
	}	
	//Als de tijdelijke array state x is en verkeerslicht 1 is op rood
	if (Temp_Array[state][0] == 1)
	{
		orByteB |= 0b00000010;
	}
	//Als de tijdelijke array state x is en verkeerslicht 1 is op oranje
	if (Temp_Array[state][0] == 2)
	{
		orByteB |= 0b00000100;
	}
	//Als de tijdelijke array state x is en verkeerslicht 1 is op groen
	if (Temp_Array[state][0] == 3)
	{
		orByteB |= 0b00001000;
	}

	//------------------------------------------------------------------------------
	//Als de tijdelijke array state x is en verkeerslicht 2 is op rood
	if (Temp_Array[state][1] == 1)
	{
		orByteA |=  0b10000000;
	}
	//Als de tijdelijke array state x is en verkeerslicht 2 is op oranje
	if (Temp_Array[state][1] == 2)
	{
		orByteA |=  0b01000000;
	}
	//Als de tijdelijke array state x is en verkeerslicht 2 is op groen
	if (Temp_Array[state][1] == 3)
	{
		orByteB |=  0b00000001;
	}

	//-------------------------------------------------------------------------------
	//Als de tijdelijke array state x is en verkeerslicht 3 is op rood
	if (Temp_Array[state][2] == 1)
	{
		orByteC |=  0b00010000;
	}
	//Als de tijdelijke array state x is en verkeerslicht 3 is op oranje
	if (Temp_Array[state][2] == 2)
	{
		orByteC |=  0b00001000;
	}
	//Als de tijdelijke array state x is en verkeerslicht 3 is op groen
	if (Temp_Array[state][2] == 3)
	{
		orByteC |=  0b00000100;
	}


	PORTA = orByteA;
	PORTB = orByteB;
	PORTC = orByteC;
}

//Functie om status sensoren te krijgen
int get_sensors()
{
	int sensors = PINA & 7;
	return ((sensors >> 2) & 1) | (sensors & 2) | ((sensors << 2) & 3);
}

//Functie om te bapelen welke state je in zit
void set_state()
{
	int sensor = get_sensors();
	switch(state)
	{
		case 1:
		{
			//Als sensor & 1 gelijk is aan 1, dan wordt het state 6
			if(sensor & 1 == 1)
			{
				state = 6;
			}
			//Als sensor & 2 gelijk is aan 2, dan wordt het state 4
			else if(sensor & 2 == 2)
			{
				state = 4;
			}
			//Als sensor & 4 gelijk is aan 4, dan wordt het state 2
			else if(sensor & 4 == 4)
			{
				state = 2;
			}
			//Anders wordt het state 1
			else
			{
				state = 1;
			}
		}
		break;
		case 2:
		{
			//Als sensor & 4 gelijk is aan 0, dan wordt het state 3
			if(sensor & 4 == 0)
			{
				state = 3;
			}
		}
		break;
		case 3:
		{
			//Als sensor & 1 gelijk is aan 1, dan wordt het state 6
			if(sensor & 1 == 1)
			{
				state = 6;
			}
			//Als sensor & 2 gelijk is aan 2, dan wordt het state 4
			else if(sensor & 2 == 2)
			{
				state = 4;
			}
			//Als sensor & 4 gelijk is aan 4, dan wordt het state 2
			else if(sensor & 4 == 4)
			{
				state = 2;
			}
			//Anders wordt het state 1
			else
			{
				state = 1;
			}
		}
		break;
		case 4:
		{
			//Als sensor & 2 gelijk is aan 0, dan wordt het state 5
			if(sensor & 2 == 0)
			{
				state = 5;
			}
		}
		break;
		case 5:
		{
			//Als sensor & 1 gelijk is aan 1, dan wordt het state 6
			if(sensor & 1 == 1)
			{
				state = 6;
			}
			//Als sensor & 2 gelijk is aan 2, dan wordt het state 4
			else if(sensor & 2 == 2)
			{
				state = 4;
			}
			//Als sensor & 4 gelijk is aan 4, dan wordt het state 2
			else if(sensor & 4 == 4)
			{
				state = 2;
			}
			//Anders wordt het state 1
			else
			{
				state = 1;
			}
		}
		break;
		case 6:
		{
			//Als sensor & 1 gelijk is aan 0, dan wordt het state 1
			if(sensor & 1 == 0)
			{
				state = 1;
			}
		}
		break;
		case 7:
		{
			//Als sensor & 1 gelijk is aan 1, dan wordt het state 6
			if(sensor & 1 == 1)
			{
				state = 6;
			}
			//Als sensor & 2 gelijk is aan 2, dan wordt het state 4
			else if(sensor & 2 == 2)
			{
				state = 4;
			}
			//Als sensor & 4 gelijk is aan 4, dan wordt het state 2
			else if(sensor & 4 == 4)
			{
				state = 2;
			}
			//Anders wordt het state 1
			else
			{
				state = 1;
			}
		}
		break;
	}
}
